#include <limits>
#include <set>

#include "map.h"

// 构造函数：初始化网格地图
// grid: 二维向量，表示网格的信息
// cross_rows: 向量，表示相交行的索引集合
GridMap::GridMap(const std::vector<std::vector<int>>& grid)
    : grid_(grid), height_(grid_.size()), width_(grid_[0].size()) {
    // 调整网格大小，用kNormal初始化所有栅格
    grid_.resize(height_, std::vector<int>(width_, GridType::kNormal));

    // 填充cross_rows_
    for (int row = 0; row < height_; row++) {
        if (grid_[row][0] == GridType::kCross) {
            cross_rows_.push_back(row);
        }
    }

    // 初始化标签，所有栅格的初始标签为(0, kUnknown)
    labels_.resize(height_, std::vector<std::pair<Zone, int>>(width_, std::make_pair(Zone::kUnknown, 0)));

    // 遍历每个栅格，根据其位置分配标签
    for (int row = 0; row < height_; row++) {
        for (int col = 0; col < width_; col++) {
            // 栅格位于第一个相交行之前，标记为左侧
            if (row >= 0 && row <= cross_rows_.front()) {
                labels_[row][col].first = Zone::kLeft;
            }
            // 栅格位于两个相交行之间，标记为中间
            else if (row >= cross_rows_.front() && row <= cross_rows_.back()) {
                labels_[row][col].first = Zone::kMiddle;
            }
            // 栅格位于最后一个相交行之后，标记为右侧
            else if (row >= cross_rows_.back() && row <= height_) {
                labels_[row][col].first = Zone::kRight;
            }

            // 栅格位于母道上，标记为可自由移动区域
            if (grid_[row][col] == GridType::kCross) {
                labels_[row][col].first = Zone::kFree;
            }
        }
    }

    for (int row = 0; row < height_; row++) {
        for (int col = 0; col < width_; col++) {
            // 当前栅格属于左侧区域，簇设置成-1
            if (labels_[row][col].first == +Zone::kLeft) {
                labels_[row][col].second = -1;
            }
            // 当前栅格属于中间区域，根据其位置和intersect_rows_的比较结果更新簇
            else if (labels_[row][col].first == +Zone::kMiddle) {
                for (int i = 0; i < cross_rows_.size(); i++) {
                    // 当找到第一个大于当前栅格y坐标的行时，簇id成i-1
                    if (cross_rows_[i] > row) {
                        labels_[row][col].second = i - 1;
                        break;
                    }
                }
            }
            // 当前栅格属于右侧区域，簇id设置为intersect_rows_的最后一个元素
            else if (labels_[row][col].first == +Zone::kRight) {
                labels_[row][col].second = cross_rows_.size() - 1;
            }
            // 当前栅格在母道上，簇id设置成-2
            else if (labels_[row][col].first == +Zone::kFree) {
                labels_[row][col].second = -2;
            }
        }
    }
}

// 贪婪算法实现目标点最优路线规划
// 参数 start: 起始点的位置
// 参数 goals: 所有目标点的位置集合
// 返回值: 返回一个整数向量，表示访问点的顺序
std::vector<int> GridMap::GreedyPlan(const Point& start, const std::vector<Point>& goals) {
    // 获取目标点的数量
    int n = goals.size();
    // 初始化路径向量用于存储访问点的顺序
    std::vector<int> path;
    // 初始化访问标记向量，用于跟踪每个点是否已被访问
    std::vector<bool> visited(n, false);

    // 找到起始点的索引
    int    current = -1;
    double minDist = std::numeric_limits<double>::max();
    // 遍历所有目标点，找到与起始点距离最近的点
    for (int i = 0; i < n; ++i) {

        double dist = Distance(start, goals[i]);
        if (dist < minDist) {
            minDist = dist;
            current = i;
        }
    }
    // 将最近的点添加到路径中并标记为已访问
    path.push_back(current);
    visited[current] = true;

    // 依次选择下一个最近的点
    for (int i = 1; i < n; ++i) {
        int next        = -1;
        int minDistance = std::numeric_limits<int>::max();

        // 遍历所有未访问的点，找到与当前点距离最近的点
        for (int j = 0; j < n; ++j) {
            if (!visited[j]) {
                int distance = Distance(goals[current], goals[j]);
                if (distance < minDistance) {
                    minDistance = distance;
                    next        = j;
                }
            }
        }
        // 将最近的点添加到路径中并标记为已访问
        path.push_back(next);
        visited[next] = true;
        // 更新当前点索引
        current = next;
    }

    return path;
}

/**
 * 计算两点之间的距离
 *
 * 此函数根据两点所在的簇和它们在网格地图中的位置计算它们之间的距离
 * 如果两点位于同一个簇且不在母道上，同时也不在同一子道，计算带有约束的曼哈顿距离
 * 否则，将直接计算两点之间的曼哈顿距离
 *
 * @param p1 第一个点的坐标
 * @param p2 第二个点的坐标
 * @return 两点之间的距离
 */
int GridMap::Distance(const Point& p1, const Point& p2) {
    int dist = 0;
    // 获取两点所在的簇
    int cluster1 = labels_[p1.y][p1.x].second;
    int cluster2 = labels_[p2.y][p2.x].second;

    // 检查两点是否位于同一个簇且不在母道上，同时也不在同一子道
    if ((cluster1 == cluster2) && (cluster1 != -2) && (GetChildIndx(p1) != GetChildIndx(p2))) {
        // 位于最左侧区域，计算两点和右侧母道的距离，以及在子道上需要移动的距离
        if (labels_[p1.y][p1.x].first == +Zone::kLeft && labels_[p2.y][p2.x].first == +Zone::kLeft) {
            int row = cross_rows_[0];
            dist    = std::abs(p1.y - row) + std::abs(p2.y - row) + std::abs(p1.x - p2.x);
        }
        // 位于最右侧区域，计算两点和左侧母道的距离，以及在子道上需要移动的距离
        else if (labels_[p1.y][p1.x].first == +Zone::kRight && labels_[p2.y][p2.x].first == +Zone::kRight) {
            int row = cross_rows_[cross_rows_.size() - 1];
            dist    = std::abs(p1.y - row) + std::abs(p2.y - row) + std::abs(p1.x - p2.x);
        }
        // 位于中间区域，计算两点和左右母道的距离，以及在子道上需要移动的距离，选择最小距离
        else if (labels_[p1.y][p1.x].first == +Zone::kMiddle && labels_[p2.y][p2.x].first == +Zone::kMiddle) {
            int left_row   = cross_rows_[cluster1];
            int right_row  = cross_rows_[cluster1 + 1];
            int left_dist  = std::abs(p1.y - left_row) + std::abs(p2.y - left_row) + std::abs(p1.x - p2.x);
            int right_dist = std::abs(p1.y - right_row) + std::abs(p2.y - right_row) + std::abs(p1.x - p2.x);
            dist           = std::min(left_dist, right_dist);
        }
    }
    // 其他情况都可以使用曼哈顿距离计算移动距离
    else {
        dist = std::abs(p1.x - p2.x) + std::abs(p1.y - p2.y);
    }

    return dist;
}